﻿#include "low-level\memory.h"
#include "Mobile.h"

static const float GRAVITATIONAL_ACCELERATION = 9.81f;

Mobile::Mobile()
{
	//__asm { int 3 }
	this->P.Reset();
	this->L.Reset();
	this->pos.Reset();
	this->q.Set(1.0f, 0.0f, 0.0f, 0.0f);
	this->m = 1.0f;
	this->Ibody = NULL;
	this->Ibodyinv = NULL;
}

Mobile::~Mobile()
{
}

OPERATOR_NEW_OVERLOAD(Mobile)

void Mobile::TestMemoryLayout()
{
	//unsigned long offset = (unsigned long)&(this->pos) - (unsigned long)this;
	//if (offset > 0)
	//{
	//	cout << "Mobile class has invalid memlayout (pos offset = " << offset << ")" << endl;
	//}
	if (((unsigned long)this & 15) != 0)
	{
		cout << "Mobile class has invalid memory layout (this)" << endl;
	}
	if (((unsigned long)&(this->pos) & 15) != 0)
	{
		cout << "Mobile class has invalid memory layout (this->pos)" << endl;
	}
	if (((unsigned long)&(this->v) & 15) != 0)
	{
		cout << "Mobile class has invalid memory layout (this->v)" << endl;
	}
	if (((unsigned long)&(this->force) & 15) != 0)
	{
		cout << "Mobile class has invalid memory layout (this->force)" << endl;
	}
}

// add a force defined in global coordinate system (eg. gravitation)
void Mobile::AddForce(Triple& f, Triple& r)
{
	// angular
	Triple tau;
	tau.CrossProduct(r, f);
	torque += tau;

	// linear
	force += f;
}

// add a force defined in local coordinate system (eg. engine force)
void Mobile::AddLocalForce(Triple& loc__f, Triple& loc__r)
{
	// angular
	Triple tau;
	tau.CrossProduct(loc__r, loc__f);
	loc__torque += tau;

	// linear
	loc__force += loc__f;
}

void Mobile::OCS_to_WCS()
{
	// loc__force => force
	Triple ___force;
	___force.Multiply(this->Rt, this->loc__force);
	force += ___force;

	// loc__torque => torque
	Triple __torque;
	__torque.Multiply(this->Rt, this->loc__torque);
	torque += __torque;
}

void Mobile::ResetForce()
{
	force.Reset();
	torque.Reset();

	loc__force.Reset();
	loc__torque.Reset();
}

void Mobile::UpdateForce()
{
	this->ResetForce();

	this->AddForce();

	this->AddLocalForce();

	this->OCS_to_WCS();

	// !! DO NOT USE LOCAL FORCE&TORQUE ANYMORE
}

void Mobile::AddGravitationalForce()
{
	force[Y] -= m * GRAVITATIONAL_ACCELERATION;
}

void Mobile::Translate(const FP_DATATYPE dt)
{
#ifdef SSSE3
	const float this__m = this->m; // TODO How to avoid this local variable?
	__asm
	{
		//int 3 ;// breakpoint

		movss xmm1,dt                          ;// load dt
        shufps xmm1,xmm1,0x00

		mov ebx,this
		mov eax,ebx                            ;// load force
		add eax,force
		movaps xmm0,xmmword ptr [eax]
		mulps xmm0,xmm1

		                ;// xmm0 contains dP

		mov eax,ebx                            ;// load P
		add eax,P
		movaps xmm2,xmmword ptr [eax]
		addps xmm0,xmm2

		movaps xmmword ptr [eax],xmm0          ;// store P

		                ;// xmm0 contains P

		movss xmm2,this__m                     ;// load m
        shufps xmm2,xmm2,0x00

		divps xmm0,xmm2

		                ;// xmm0 contains speed

		mov eax,ebx
		add eax,v
		movaps xmmword ptr [eax],xmm0          ;// store speed

		mulps xmm0,xmm1

		                ;// xmm0 contains dpos

		mov eax,ebx
		add eax,pos                             ;// load pos
		movaps xmm2,xmmword ptr [eax]
		addps xmm0,xmm2

		movaps xmmword ptr [eax],xmm0           ;// store pos
	}
#else
	P.Integrate(force, dt); // update linear impulse (P)
	v = P / m;
	pos.Integrate(v, dt);
#endif
}

void Mobile::Rotate(const FP_DATATYPE dt)
{
	this->L.Integrate(this->torque, dt); // update angular impulse (L)

	// update rotation (q)
	Quaternion dq;
	this->Iinv = this->R * (*this->Ibodyinv) * this->Rt; // matrix-only operations
	this->omega = this->Iinv * this->L;
	dq = this->omega * this->q;

	this->q.Integrate(dq, dt);
}

// not tested
void Mobile::Move(const FP_DATATYPE dt)
{
	this->R = this->q;
	this->Rt.Transpose(this->R);

	this->UpdateForce();

	this->Translate(dt);

	this->Rotate(dt);

	this->q.Normalize();
}

// tested
void Mobile::Move__rot_test(const FP_DATATYPE dt)
{
	this->R = this->q;
	this->Rt.Transpose(this->R);

	this->Rotate(dt);

	this->q.Normalize();

//	__asm { int 3 }
}

void Mobile::SetDefaultInertia()
{
	this->m = 1.0000000e+003f;

	/* DOES NOT WORK
	this->Ibody = new Matrix();
	this->Ibody->operator[](0).Set(1.6666667e+004f, 0.0000000e+000f, 0.0000000e+000f);
	this->Ibody->operator[](1).Set(0.0000000e+000f, 1.6666667e+004f, 0.0000000e+000f);
	this->Ibody->operator[](2).Set(0.0000000e+000f, 0.0000000e+000f, 1.6666667e+004f);

	this->Ibodyinv = new Matrix();
	this->Ibodyinv->operator[](0).Set(6.0000000e-005f, 0.0000000e+000f, -0.0000000e+000f);
	this->Ibodyinv->operator[](1).Set(0.0000000e+000f, 6.0000000e-005f, -0.0000000e+000f);
	this->Ibodyinv->operator[](2).Set(0.0000000e+000f, 0.0000000e+000f, 6.0000000e-005f);
	*/

	static Matrix defaultIbody;
	static Matrix defaultIbodyinv;

	defaultIbody.operator[](0).Set(1.6666667e+004f, 0.0000000e+000f, 0.0000000e+000f);
	defaultIbody.operator[](1).Set(0.0000000e+000f, 1.6666667e+004f, 0.0000000e+000f);
	defaultIbody.operator[](2).Set(0.0000000e+000f, 0.0000000e+000f, 1.6666667e+004f);

	defaultIbodyinv.operator[](0).Set(6.0000000e-005f, 0.0000000e+000f, -0.0000000e+000f);
	defaultIbodyinv.operator[](1).Set(0.0000000e+000f, 6.0000000e-005f, -0.0000000e+000f);
	defaultIbodyinv.operator[](2).Set(0.0000000e+000f, 0.0000000e+000f, 6.0000000e-005f);

	this->Ibody = &defaultIbody;
	this->Ibodyinv = &defaultIbodyinv;
}